#include <LPC22xx.h>
#include "uart.h"
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

int UARTBufferCount;
char UARTBuffer[500];

void UART_IRQHandler (void) __irq 
{
	// read all data
	while( U0LSR & 0x01 )
	{
		unsigned char data = U0RBR;
		UARTBuffer[UARTBufferCount] = data;
		if(UARTBufferCount<(500-1)) UARTBufferCount++;
	}

	VICVectAddr = 0;                      /* Acknowledge Interrupt              */	
}

void UARTDriver::Init()
{	
	UARTBufferCount = 0;
	RXBufPosition = 0;
	PWMValue = 1000;

	// set UART pins
	PINSEL0 |= 0x05; // set bit 1 and 3
	
	// set 8N1 + activate divisor latch
	U0LCR = 0x83;   /* 8 bits, no Parity, 1 Stop bit */
	// set baud rate 57600 -> pclk = 16MHz
	U0DLM = 0x00; //MSB	 
	U0DLL = 0x21; //LSB
	//U0FDR=0x00000000;
	// remove divisor latch
	U0LCR = 0x03;
	// activate FIFO
	U0FCR = 0x47;	// 14 bytes fifo + enable + reset + int on 4 bytes in FIFO
	
	// set interrupts			 VICVectCntl is used as 10, because of priority: (0-highest, 15-lowest)
	VICVectCntl10  = 32 + 6;                          /* enable and set ID to 6 (uart0 int) */
	VICVectAddr10  = (unsigned long)UART_IRQHandler;/* Set Interrupt Vector        */
	VICIntEnable  |= (1 << 6);                   /* Enable Uart0 Interrupt     */
	U0IER = 0x00000001;		// enable RX int
}			 

void UARTDriver::SendChar(char ch)
{
	while( !(U0LSR & 0x20) ); // wait for tx is cleared
	U0THR = ch;
}
									
void UARTDriver::SendData(char* buffer)
{
	SendData(buffer, strlen(buffer));
}    

void UARTDriver::SendData(char* buffer, int length)
{
	for(int i=0; i!= length; i++)
	{
		SendChar(*buffer);
		buffer++;
	}
}

int UARTDriver::ReceiveCommand(char* buffer) // returns message length
{
	int count;
	VICIntEnClr |= (1 << 6);
	memcpy(buffer, UARTBuffer, UARTBufferCount);
	count = UARTBufferCount;
	UARTBufferCount = 0; 
	VICIntEnable |= (1 << 6);

	return count;
}

int UARTDriver::ParseCommand(char* input) // returns message length
{
	for(int i=0; i!= strlen(input); i++)
	{
		char data = input[i];
		if( data == '\r' )
		{
			// end of command
			int size = RXBufPosition;
			RXBufPosition = 0;
			RXBuffer[size] = 0; // null terminate

			//if( CheckChecksum(RXBuffer) == true )
			{			
				ExecuteCommand(RXBuffer, size);
			}
		}
		else
		{
			if( RXBufPosition >= 200 || data == '$')
			{
				// overrun or RESET
				RXBufPosition = 0;
			}
			RXBuffer[RXBufPosition] = data;
			RXBufPosition++;
		}
	}
	return 0;
}

void UARTDriver::ExecuteCommand(char* input, int size)
{
	// parse command
	char* token = strtok( input, ",");
	if( !strcmp(token, "$PWM") )
	{		
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Sat Used
			int pwm = 0;
			pwm = atoi(token);
			if( pwm > 500 && pwm < 3000 ) PWMValue = pwm;
			//printf("PWM: %dus\r\n", PWMValue);
		}
		/*token = strtok(NULL, ",");
		if( token != NULL )
		{
			// HDOP
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// msl
			m_alt = atof(token);
		} */
	}
}
